/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#pragma once

#include "Chain.h"

namespace Robotic { namespace Mechanism
{
	using namespace Robotic::Math;

	/*
	The implementation of the impedance controller is made of two section:
	the first one is the generation of a compliance trajectory and the second one 
	used a position controller to ensure the end effector follow the compliance
	trajectory (We recommended to used the resolve acceleration controller 
	scheme, implemented in the class Resolved_acc).
	*/
	class ROBOTIC_MECHANISM_API Con_Impedance
	{
	public:
		Con_Impedance();
		Con_Impedance(const Chain & robot, const ColumnVector & Mp_,
			const ColumnVector & Dp_, const ColumnVector & Kp_,
			const ColumnVector & Mo_, const ColumnVector & Do_, 
			const ColumnVector & Ko_);
		void Set_Mp(const ColumnVector & Mp_);
		void Set_Mp(const double MP_i, const int i);
		void Set_Dp(const ColumnVector & Dp_);
		void Set_Dp(const double Dp_i, const int i);
		void Set_Kp(const ColumnVector & Kp_);
		void Set_Kp(const double Kp_i, const int i);
		void Set_Mo(const ColumnVector & Mo_);
		void Set_Mo(const double Mo_i, const int i);
		void Set_Do(const ColumnVector & Do_);
		void Set_Do(const double Do_i, const int i);
		void Set_Ko(const ColumnVector & Ko_);
		void Set_Ko(const double Ko_i, const int i);

		/*!
		@brief Generation of a compliance trajectory
		@param pdpp: desired end effector acceleration.
		@param pdp: desired end effector velocity.
		@param pd: desired end effector position.
		@param wdp: desired end effector angular acceleration.
		@param wd: desired end effector angular velocity.
		@param qd: desired quaternion.
		@param f: end effector contact force.
		@param n: end effector contact moment.
		@param dt: time frame.

		The quaternion -q represents exactly the same rotation as the quaternion q. The 
		temporay quaternion, _quat, is quatd plus a sign correction. It is customary to 
		choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). 
		This choice avoids extra spinning caused by the interpolated rotations.
		*/
		void Control(const ColumnVector & pdpp, const ColumnVector & pdp,
			const ColumnVector & pd, const ColumnVector & wdp,
			const ColumnVector & wd, const Quaternion & qd,
			const ColumnVector & f, const ColumnVector & n,
			const double dt);

		Quaternion _qc;          //!< Compliant frame quaternion.
		Quaternion _qcp;         //!< Compliant frame quaternion derivative.
		Quaternion _qcp_prev;    //!< Previous value of _qcp.
		Quaternion _qcd;         //!< Orientation error (betweem compliant and desired frame) quaternion.
		Quaternion _quat;         //!< Temporary quaternion.
		ColumnVector _pc;        //!< Compliant position.
		ColumnVector _pcp;       //!< Compliant velocity.
		ColumnVector _pcpp;      //!< Compliant acceleration.
		ColumnVector _pcp_prev;  //!< Previous value of _pcp.
		ColumnVector _pcpp_prev; //!< Previous value of _pcpp.
		ColumnVector _pcd;       //!< Difference between _pc and desired position.
		ColumnVector _pcdp;      //!< Difference between _pcp and desired velocity.
		ColumnVector _wc;       //!< Compliant angular velocity.
		ColumnVector _wcp;       //!< Compliant angular acceleration.
		ColumnVector _wcp_prev;  //!< Previous value of _wcp.
		ColumnVector _wcd;       //!< Difference between _wc and desired angular velocity.
	private:
		Matrix _Mp;   //!< Translational impedance inertia matrix.
		Matrix _Dp;   //!< Translational impedance damping matrix.
		Matrix _Kp;   //!< Translational impedance stifness matrix.
		Matrix _Mo;   //!< Rotational impedance inertia matrix.
		Matrix _Do;   //!< Rotational impedance damping matrix.
		Matrix _Ko;   //!< Rotational impedance stifness matrix.
		Matrix _Ko_prime;     //!< Modified rotational impedance stifness matrix.
	};


}}